#define ESP8266_ENV
//#define ACTION_DEBUG
#include <Arduino.h>
#include <Ticker.h>
#include "LightClient.h"
#include "Quadruped.h"

// wifi ssid与密码
char *ssid = "test0";
char *passphrase = "12345687";
// 服务端 地址与端口
char *host = "192.168.137.1";
uint16_t serverPost = 8081;
// 设备 用户名密码
char *userName = "light";
char *passwd = "light";

LightClient lightClient;
MsgHeader msgHeader;

Ticker ticker;

Quadruped quadruped = Quadruped(
        HipJoint(0, 23, 79), KneeJoint(1, 70, 30),
        HipJoint(2, 17, 73), KneeJoint(3, 40, 80),
        HipJoint(5, 6, 62), KneeJoint(4, 46, 6),
        HipJoint(6, 16, 72), KneeJoint(7, 35, 75)
);


// opCode 操作表
void debugOp();

void servoTurn();

void action();

void all();

void (*opTab[5])() = {debugOp, servoTurn, action, all};

/**
 * 控制动作的线程，定时器，线程中不能使用delay.
 * 所以每一步需要切换执行内容
 * */
void quadrupedActionTick() {
#ifdef ACTION_DEBUG
    Serial.println("[tick] do tick ");
#endif


    int action = quadruped.getQuadrupedAction();
    if (action == -1) {
        return;
    }
    quadruped.doAsyncAction(action);
}

void setup() {
    Serial.begin(115200);
    while (!Serial) {
    }

    Serial.println("init quadruped!");

    // 8266默认 4 和 5 其它引脚也可指定
    quadruped.init(SDA, SCL);
    ConnectInfo connectInfo;
    connectInfo.ssid = ssid;
    connectInfo.passphrase = passphrase;
    connectInfo.host = host;
    connectInfo.serverPost = serverPost;
    connectInfo.userName = userName;
    connectInfo.passwd = passwd;
    connectInfo.attrCount = 2;
    connectInfo.devAttr = "version:1.0\ndevType:quadruped\n";

    Serial.println("start handle");
    ticker.attach_ms(step_delay, quadrupedActionTick);
    if(lightClient.connectServer(&connectInfo)){
        Serial.println("init success");
    }

}

void loop() {

    int result = lightClient.readMsgHeader(&msgHeader);

    if (result == MSG_FIALED) {
        return;
    }
    int opCode = msgHeader.opCode;
    Serial.printf("cmdId:%d,opCode:%d\n", msgHeader.cmdIdVal, opCode);
    opTab[opCode]();
}

void servoTurn() {
    int target = lightClient.readInt(' ');
    int pos = lightClient.readInt('\n');
    Serial.print("target:");
    Serial.print(target);
    Serial.print("src_pos:");
    Serial.print(pos);
    //  现需要把 0~100的值映射到 BASE_POS ~ END_POS
    // y= BASE_POS + kx ；k= （END_POS - BASE_POS） / 100;
    // int类型除法无法保留小数  可以先乘后除 得 下式
    pos = (END_POS - BASE_POS) * pos / 100 + BASE_POS;
    Serial.print(",pos:");
    Serial.println(pos);
    quadruped.turn(target, pos);
}

void action() {
    int action = lightClient.readInt('\n');
    quadruped.setQuadrupedAction(action);
    Serial.printf("action is %d\n", action);
}

void all() {
    int action = lightClient.readInt('\n');
    void (Quadruped::*allPinAction)() = quadruped.allPinActionTab[action];
    (quadruped.*allPinAction)();
}

void debugOp() {
    Serial.println("rec:" + lightClient.readStringUntil('\n'));
}
